import rclpy
import cv2
import cv_bridge
import numpy as np
from rclpy.node import Node
from sensor_msgs.msg import Image
from std_msgs.msg import Int32  # 使用标准消息类型
import time

class Follower(Node):
    def __init__(self):
        super().__init__('line_follower')
        self.get_logger().info("Start line follower.")
        
        self.bridge = cv_bridge.CvBridge()
        
        self.image_sub = self.create_subscription(Image, 'image_raw1', self.image_callback, 10)
        self.qr_data_pub = self.create_publisher(Int32, '/sign_foxglove.data', 10)  # 发布到正确的路径
        
        self.qrDecoder = cv2.QRCodeDetector()  # 创建 qrCodeDetector 对象
        self.last_published_time = time.time()
        self.publish_interval = 1.0  # 设置消息发布间隔为1秒
        self.get_logger().info("Initialization complete.")

    def image_callback(self, msg):
        self.get_logger().info("Image callback triggered.")
        image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
        
        # 检测并解码二维码
        data, bbox, rectifiedImage = self.qrDecoder.detectAndDecode(image)
        current_time = time.time()
        if data and (current_time - self.last_published_time) >= self.publish_interval:
            self.last_published_time = current_time
            self.get_logger().info(f"QR Code detected: {data}")
            # 根据二维码数据发送不同的值到指定话题
            qr_msg = Int32()
            if data == 'ClockWise':
                qr_msg.data = 3
                self.get_logger().info(f"Preparing to publish data: {qr_msg.data}")
            elif data == 'AntiClockWise':
                qr_msg.data = 4
                self.get_logger().info(f"Preparing to publish data: {qr_msg.data}")
            else:
                qr_msg.data = 2  # 其他情况处理为已到达二维码区域
                self.get_logger().info(f"Preparing to publish data: {qr_msg.data}")
            
            # 发布消息
            self.qr_data_pub.publish(qr_msg)
            self.get_logger().info(f"Published data: {qr_msg.data} at {time.time()}")

def main(args=None):
    rclpy.init(args=args)
    follower = Follower()
    rclpy.spin(follower)
    follower.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
